#include "fishbot_correct_pose/fishbot_correct_pose.h"
#include <fstream>
void CorrectPose::processTF(const std::vector<geometry_msgs::msg::TransformStamped> &transforms)
{

    bool has_tag = false; // 标记是否存在包含 "tag" 的子坐标系

    for (const auto &transform : transforms)
    {
        const std::string &child_frame = transform.child_frame_id;

        // 检查是否包含 "tag"
        if (child_frame.find("tag") != std::string::npos)
        {
            has_tag = true;                     // 找到包含 "tag" 的子坐标系
            tag_frames_.clear();                // 清空之前的内容
            tag_frames_.push_back(child_frame); // 只保留最后一个包含 "tag" 的子坐标系
        }
    }

    // 如果没有任何包含 "tag" 的子坐标系，则清空 tag_frames_
    if (!has_tag)
    {
        tag_frames_.clear();
    }
}

void CorrectPose::Callback()
{
    std::ifstream fin("/home/fwy/nav_ws/src/fishbot_apriltag_detect/tag_save_map/tag_tf_map.yaml");
    YAML::Node yaml_node;

    if (fin.is_open())
    {
        yaml_node = YAML::Load(fin);
        fin.close();
    }

    if (correct_pose_flag_ == true && previous_correct_pose_flag_ == false)
    {
        // 记录第一次切换 flag 的时间
        flag_switch_time_ = this->now();
        RCLCPP_INFO(this->get_logger(), "CorrectPose flag switched from false to true at time: %f", flag_switch_time_.seconds());
    }
    if (correct_pose_flag_ && (this->now() - flag_switch_time_).seconds() < 2.0) // 阈值为 2 秒
    {
        if (!tag_frames_.empty())
        {
            RCLCPP_INFO(this->get_logger(), "use %s start to correct pose and publish map->odom", tag_frames_[0].c_str());
            pubInitialPose(yaml_node);
            pubMapOdomTransform();
        }
    }

    previous_correct_pose_flag_ = correct_pose_flag_;
}

void CorrectPose::amclCallBack(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
{
    amcl_pose_ = *msg;
}

void CorrectPose::pubInitialPose(YAML::Node yaml_node)
{
    std::string child_frame = tag_frames_[0];
    // 获取 base_link 到 tag 的变换
    auto base2tag = tf_buffer_->lookupTransform(
        "base_link",
        child_frame,
        tf2::TimePointZero);

    // 获取 map 到 tag 的变换 真值
    geometry_msgs::msg::TransformStamped map2tag_real;
    map2tag_real.transform.translation.x = yaml_node[child_frame]["translation"]["x"].as<double>();
    map2tag_real.transform.translation.y = yaml_node[child_frame]["translation"]["y"].as<double>();
    map2tag_real.transform.translation.z = yaml_node[child_frame]["translation"]["z"].as<double>();
    map2tag_real.transform.rotation.x = yaml_node[child_frame]["rotation"]["x"].as<double>();
    map2tag_real.transform.rotation.y = yaml_node[child_frame]["rotation"]["y"].as<double>();
    map2tag_real.transform.rotation.z = yaml_node[child_frame]["rotation"]["z"].as<double>();
    map2tag_real.transform.rotation.w = yaml_node[child_frame]["rotation"]["w"].as<double>();
    map2tag_real.header.frame_id = "map";
    map2tag_real.child_frame_id = child_frame;
    map2tag_real.header.stamp = this->now();

    // 将 geometry_msgs::TransformStamped 转换为 tf2::Transform
    tf2::Transform tf_base2tag, tf_map2tag_real;
    tf2::fromMsg(base2tag.transform, tf_base2tag);
    tf2::fromMsg(map2tag_real.transform, tf_map2tag_real);

    // 计算 base2tag 的逆变换
    tf2::Transform tf_tag2base = tf_base2tag.inverse();

    // 计算 map 到 base_link 的变换
    tf2::Transform tf_map2base = tf_map2tag_real * tf_tag2base;

    // 将结果转换回 geometry_msgs::TransformStamped
    geometry_msgs::msg::TransformStamped map2base;
    map2base.transform = tf2::toMsg(tf_map2base);
    map2base.header.frame_id = "map";
    map2base.child_frame_id = "base_link";
    map2base.header.stamp = this->now();
    // 发布或使用 map2base 变换
    RCLCPP_INFO(this->get_logger(), "Computed map to base_link transform:");
    RCLCPP_INFO(this->get_logger(), "Translation: x=%.3f, y=%.3f, z=%.3f",
                map2base.transform.translation.x,
                map2base.transform.translation.y,
                map2base.transform.translation.z);
    RCLCPP_INFO(this->get_logger(), "Rotation: x=%.3f, y=%.3f, z=%.3f, w=%.3f",
                map2base.transform.rotation.x,
                map2base.transform.rotation.y,
                map2base.transform.rotation.z,
                map2base.transform.rotation.w);
    // 发布 map2base 变换到/initialpose话题
    geometry_msgs::msg::PoseWithCovarianceStamped pose_with_cov;
    pose_with_cov.header.frame_id = "map";
    pose_with_cov.header.stamp = this->now();
    pose_with_cov.pose.pose.position.x = map2base.transform.translation.x;
    pose_with_cov.pose.pose.position.y = map2base.transform.translation.y;
    pose_with_cov.pose.pose.position.z = map2base.transform.translation.z;
    pose_with_cov.pose.pose.orientation.x = map2base.transform.rotation.x;
    pose_with_cov.pose.pose.orientation.y = map2base.transform.rotation.y;
    pose_with_cov.pose.pose.orientation.z = map2base.transform.rotation.z;
    pose_with_cov.pose.pose.orientation.w = map2base.transform.rotation.w;
    pose_with_cov.pose.covariance[0] = 0.04; // 设置协方差
    pose_with_cov.pose.covariance[7] = 0.04;
    pose_with_cov.pose.covariance[35] = 0.04;
    pub_initial_pose_->publish(pose_with_cov);
}

void CorrectPose::pubMapOdomTransform()
{
    // 检查 amcl_pose_ 是否已接收到数据
    if (amcl_pose_.header.frame_id.empty())
    {
        RCLCPP_WARN(this->get_logger(), "AMCL pose not received yet, cannot publish map->odom transform.");
        return;
    }
    geometry_msgs::msg::TransformStamped geomsg_odom_base_link;
    // 获取 base_link 到 tag 的变换
    try
    {
        geomsg_odom_base_link = tf_buffer_->lookupTransform(
            "odom",
            "base_footprint",
            amcl_pose_.header.stamp,
            rclcpp::Duration::from_nanoseconds(100000));
    }
    catch (const tf2::TransformException &ex)
    {
        RCLCPP_WARN(this->get_logger(), "Failed to get odom to base_link transform: %s", ex.what());
        return;
    }

    // 将amcl_pose_(map->base_link的变换)转换为 tf2::Transform
    tf2::Transform tf_map_base_link;
    tf_map_base_link.setOrigin(tf2::Vector3(amcl_pose_.pose.pose.position.x,
                                            amcl_pose_.pose.pose.position.y,
                                            amcl_pose_.pose.pose.position.z));
    tf_map_base_link.setRotation(tf2::Quaternion(amcl_pose_.pose.pose.orientation.x,
                                                 amcl_pose_.pose.pose.orientation.y,
                                                 amcl_pose_.pose.pose.orientation.z,
                                                 amcl_pose_.pose.pose.orientation.w));
    // 将 tf_odom_base_link_ 转换为 tf2::Transform
    tf2::Transform tf_odom_base_link;
    tf_odom_base_link.setOrigin(tf2::Vector3(geomsg_odom_base_link.transform.translation.x,
                                             geomsg_odom_base_link.transform.translation.y,
                                             geomsg_odom_base_link.transform.translation.z));
    tf_odom_base_link.setRotation(tf2::Quaternion(geomsg_odom_base_link.transform.rotation.x,
                                                  geomsg_odom_base_link.transform.rotation.y,
                                                  geomsg_odom_base_link.transform.rotation.z,
                                                  geomsg_odom_base_link.transform.rotation.w));

    // 根据tf_map_to_base_link和tf_odom_base_link_计算map->odom的变换
    tf2::Transform tf_map_to_odom = tf_map_base_link * tf_odom_base_link.inverse();

    // 将变换转换为 geometry_msgs::msg::TransformStamped
    geometry_msgs::msg::TransformStamped map_to_odom;
    map_to_odom.transform.translation.x = tf_map_to_odom.getOrigin().x();
    map_to_odom.transform.translation.y = tf_map_to_odom.getOrigin().y();
    map_to_odom.transform.translation.z = tf_map_to_odom.getOrigin().z();
    map_to_odom.transform.rotation.x = tf_map_to_odom.getRotation().x();
    map_to_odom.transform.rotation.y = tf_map_to_odom.getRotation().y();
    map_to_odom.transform.rotation.z = tf_map_to_odom.getRotation().z();
    map_to_odom.transform.rotation.w = tf_map_to_odom.getRotation().w();
    map_to_odom.header.frame_id = "map";
    map_to_odom.child_frame_id = "odom";
    map_to_odom.header.stamp = amcl_pose_.header.stamp;

    // 将变换封装到 TFMessage 中
    tf2_msgs::msg::TFMessage tf_map_odom;
    tf_map_odom.transforms.push_back(map_to_odom);
    // if (odom_.twist.twist.angular.z < 0.5)
    // 发布 TF 变换
    pub_tf_->publish(tf_map_odom);

    RCLCPP_INFO(this->get_logger(), "Published map->odom transform.");
}

void CorrectPose::handleTriggerService(const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
                                       std::shared_ptr<std_srvs::srv::Trigger::Response> response)
{
    if (!tag_frames_.empty())
    {
        std::ifstream fin("/home/fwy/nav_ws/src/fishbot_apriltag_detect/tag_save_map/tag_tf_map.yaml");
        YAML::Node yaml_node;

        if (fin.is_open())
        {
            yaml_node = YAML::Load(fin);
            fin.close();
        }

        RCLCPP_INFO(this->get_logger(), "Service triggered: publishing initial pose.");
        pubInitialPose(yaml_node);
        response->success = true;
        response->message = "Initial pose published successfully.";
    }
    else
    {
        response->success = false;
        response->message = "No tag frames available to publish initial pose.";
        RCLCPP_WARN(this->get_logger(), "Service triggered but no tag frames available.");
    }
}

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<CorrectPose>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}